package com.jhc.drone.communication.datagram.command;

import com.jhc.drone.communication.enums.PtzDirectEnum;
import com.jhc.drone.communication.enums.PtzZoomEnum;
import com.jhc.drone.communication.mavlink.MAVLinkPacket;
import com.jhc.drone.communication.mavlink.Messages.MAVLinkMessage;
import com.jhc.drone.communication.mavlink.common.msg_command_int;
import com.jhc.drone.communication.mavlink.common.msg_command_long;
import com.jhc.drone.communication.mavlink.common.msg_rc_channels_raw;
import com.jhc.drone.communication.mavlink.enums.MAV_CMD;
import com.jhc.drone.communication.mission.MissionBuilder;
import com.jhc.drone.communication.vo.CommandVo;
import com.jhc.drone.communication.vo.PtzVo;
public class Command {

    public static byte[] buildCommand(CommandVo vo) {
        MAVLinkMessage cmd;
        switch (vo.getWsReqEnum()) {
            case ARM:
                vo.setParam1(1);
                vo.setCommand(MAV_CMD.MAV_CMD_COMPONENT_ARM_DISARM);
                vo.setParam2(2989);
                cmd = new msg_command_long(vo);
                break;
            case TAKE_OFF:
                vo.setParam5(40.08081f); // 经度
                vo.setParam6(116.5726f); // 纬度
                vo.setParam7(50); // 起飞高度
                vo.setCommand(MAV_CMD.MAV_CMD_NAV_TAKEOFF);
                cmd = new msg_command_long(vo);
                break;
            case LAND:
                vo.setCommand(MAV_CMD.MAV_CMD_NAV_LAND);
                cmd = new msg_command_int(vo);
                break;
            case RETURN:
                vo.setCommand(MAV_CMD.MAV_CMD_NAV_RETURN_TO_LAUNCH);
                cmd = new msg_command_long(vo);
                break;
            case SET_MODE:
                vo.setCommand(MAV_CMD.MAV_CMD_DO_SET_MODE);
                vo.setParam1(1);
                cmd = new msg_command_long(vo);
                break;
            case MOVE_CONTROLLER:
                // PWM: 1040-1500-1920
                // RC_MAP_PITCH：前进后退/2
                // RC_MAP_ROLL: 左右/1
                // RC_MAP_YAW: 左右偏转/4
                // RC_MAP_THROTTLE：上升下降/3
                msg_rc_channels_raw rcMessage = new msg_rc_channels_raw();
                rcMessage.time_boot_ms = 14400000;
                rcMessage.chan1_raw = vo.startRc ? vo.getRcVo().getChannel1() : 1480;
                rcMessage.chan2_raw = vo.startRc ? vo.getRcVo().getChannel2() : 1480;
                rcMessage.chan3_raw = vo.startRc ? vo.getRcVo().getChannel3() : 1480;
                rcMessage.chan4_raw = vo.startRc ? vo.getRcVo().getChannel4() : 1480;
                rcMessage.sysid = vo.getSysId();
                rcMessage.compid = 1;
                rcMessage.port = 1;
                rcMessage.rssi = 100;
                MAVLinkPacket pack = rcMessage.pack();
                return pack.encodePacket();
            case MISSION_CONTINUE:
                vo.setParam1(1);
                vo.setCommand(MAV_CMD.MAV_CMD_DO_PAUSE_CONTINUE);
                cmd = new msg_command_long(vo);
                break;
            case PTZ_PITCH:
                PtzVo ptz = vo.getPtzVo();
                vo.setParam1(ptz.getPitchAngle()); // 云台俯仰角
                vo.setParam2(ptz.getDirectAngle()); // 云台方向角
                vo.setParam3(ptz.getParam3());// 默认0.5
                vo.setParam4(ptz.getParam4());// 默认0.5
                vo.setParam5(ptz.getDirection() != null && ptz.getDirection() == PtzDirectEnum.REST ? 5 : 0); // 回中：5，垂直向下：0
                vo.setCommand(MAV_CMD.MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW);
                cmd = new msg_command_long(vo);
                break;
            case PTZ_ZOOM:
                PtzVo ptzVo = vo.getPtzVo();
                vo.setParam1(ptzVo.getBigger() != null ? 0 : 2f); // param2为1或者-1的时候该值0,其他时候该值为2
                vo.setParam2(ptzVo.getBigger() == null ? ptzVo.getZoomMultiple() : ptzVo.getBigger() == PtzZoomEnum.BIGGER ? 1 : -1); // -1:变焦缩小，1:变焦放大，其他：变焦倍数
                vo.setParam3(ptzVo.getZoomSpeed()); //变焦速度
                vo.setCommand(MAV_CMD.MAV_CMD_SET_CAMERA_ZOOM);
                cmd = new msg_command_long(vo);
                break;
            case CLEAR_MISSION:
                MissionBuilder missionBuilder2 = new MissionBuilder();
                return missionBuilder2.clearAllMission(vo);
            case PUSH_SERIAL_NUMBER:
                vo.setCommand(MAV_CMD.MAV_CMD_DO_PUSH_SERIAL_NUMBER);
                cmd = new msg_command_long(vo);
                break;
            default:
                cmd = new msg_command_long(); 
                break;
        }
        MAVLinkPacket pack = cmd.pack();
        return pack.encodePacket();
    }

}
